/*
  June 2012

  BaseFlightPlus Rev -

  An Open Source STM32 Based Multicopter

  Includes code and/or ideas from:

  1)AeroQuad
  2)BaseFlight
  3)CH Robotics
  4)MultiWii
  5)S.O.H. Madgwick

  Designed to run on Naze32 Flight Control Board

  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include "board.h"

///////////////////////////////////////////////////////////////////////////////

// I2C2
// SCL  PB10
// SDA  PB11

static I2C_TypeDef *I2Cx;
static void i2c_er_handler(void);
static void i2c_ev_handler(void);
static void i2cUnstick(void);

void I2C1_ER_IRQHandler(void)
{
	i2c_er_handler();
}

void I2C1_EV_IRQHandler(void)
{
	i2c_ev_handler();
}

void I2C2_ER_IRQHandler(void)
{
	i2c_er_handler();
}

void I2C2_EV_IRQHandler(void)
{
	i2c_ev_handler();
}


#define I2C_DEFAULT_TIMEOUT 30000
static volatile uint16_t i2cErrorCount = 0;

static volatile bool busy;

static volatile uint8_t addr;
static volatile uint8_t reg;
static volatile uint8_t bytes;
static volatile uint8_t writing;
static volatile uint8_t reading;
static volatile uint8_t *write_p;
static volatile uint8_t *read_p;

///////////////////////////////////////////////////////////////////////////////

static void i2c_er_handler(void)
{
	volatile uint32_t SR1Register, SR2Register;

	SR1Register = I2Cx->SR1;                                             //Read the I2C1 status register

	if (SR1Register & 0x0F00)   //an error
	{
		// I2C1error.error = ((SR1Register & 0x0F00) >> 8);              //save error
		// I2C1error.job = job;    //the task
	}

	if (SR1Register & 0x0700)                                            //If AF, BERR or ARLO, abandon the current job and commence new if there are jobs

	{
		SR2Register = I2Cx->SR2;                                         //read second status register to clear ADDR if it is set (note that BTF will not be set after a NACK)
		I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE);                         //disable the RXNE/TXE interrupt - prevent the ISR tailchaining onto the ER (hopefully)

		if (!(SR1Register & 0x0200) && !(I2Cx->CR1 & 0x0200))            //if we dont have an ARLO error, ensure sending of a stop
		{
			if (I2Cx->CR1 & 0x0100)                                      //We are currently trying to send a start, this is very bad as start,stop will hang the peripheral
			{
				while (I2Cx->CR1 & 0x0100);                              //wait for any start to finish sending

				I2C_GenerateSTOP(I2Cx, ENABLE);                          //send stop to finalise bus transaction

				while (I2Cx->CR1 & 0x0200);                              //wait for stop to finish sending

				i2cInit(I2Cx);                                           //reset and configure the hardware
			}

			else
			{
				I2C_GenerateSTOP(I2Cx, ENABLE);                          //stop to free up the bus
				I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE);    //Disable EVT and ERR interrupts while bus inactive
			}
		}
	}

	I2Cx->SR1 &= ~0x0F00;                                                //reset all the error bits to clear the interrupt
	busy = 0;
}

///////////////////////////////////////////////////////////////////////////////

bool i2cWriteBuffer(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data)
{
	uint8_t i;
	uint8_t my_data[16];
	uint32_t timeout = I2C_DEFAULT_TIMEOUT;

	addr = addr_ << 1;
	reg = reg_;
	writing = 1;
	reading = 0;
	write_p = my_data;
	read_p = my_data;
	bytes = len_;
	busy = 1;

	if (len_ > 16)
		return false;                                                    //too long

	for (i = 0; i < len_; i++)
		my_data[i] = data[i];

	if (!(I2Cx->CR2 & I2C_IT_EVT))                                       //if we are restarting the driver
	{
		if (!(I2Cx->CR1 & 0x0100))                                       //ensure sending a start
		{
			while (I2Cx->CR1 & 0x0200)
			{
				;   //wait for any stop to finish sending
			}

			I2C_GenerateSTART(I2Cx, ENABLE);                             //send the start for the new job
		}

		I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, ENABLE);             //allow the interrupts to fire off again
	}

	while (busy && --timeout > 0);

	if (timeout == 0)
	{
		i2cErrorCount++;
		i2cInit(I2Cx);                                                   //reinit peripheral + clock out garbage
		return false;
	}

	return true;
}

///////////////////////////////////////////////////////////////////////////////

bool i2cWrite(uint8_t addr_, uint8_t reg_, uint8_t data)
{
	return i2cWriteBuffer(addr_, reg_, 1, &data);
}

///////////////////////////////////////////////////////////////////////////////

bool i2cRead(uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t *buf)
{
	uint32_t timeout = I2C_DEFAULT_TIMEOUT;

	addr = addr_ << 1;
	reg = reg_;
	writing = 0;
	reading = 1;
	read_p = buf;
	write_p = buf;
	bytes = len;
	busy = 1;

	if (!(I2Cx->CR2 & I2C_IT_EVT))                                       //if we are restarting the driver
	{
		if (!(I2Cx->CR1 & 0x0100))                                       //ensure sending a start
		{
			while (I2Cx->CR1 & 0x0200)
			{
				;   //wait for any stop to finish sending
			}

			I2C_GenerateSTART(I2Cx, ENABLE);                             //send the start for the new job
		}

		I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, ENABLE);             //allow the interrupts to fire off again
	}

	while (busy && --timeout > 0);

	if (timeout == 0)
	{
		i2cErrorCount++;                                                 // reinit peripheral + clock out garbage
		i2cInit(I2Cx);
		return false;
	}

	return true;
}

///////////////////////////////////////////////////////////////////////////////

void i2c_ev_handler(void)
{
	static uint8_t subaddress_sent, final_stop;                          //flag to indicate if subaddess sent, flag to indicate final bus condition
	static int8_t index;                                                 //index is signed -1==send the subaddress

	uint8_t SReg_1 = I2Cx->SR1;                                          //read the status register here

	if (SReg_1 & 0x0001)                                                 //we just sent a start - EV5 in ref manual
	{
		I2Cx->CR1 &= ~0x0800;                                            //reset the POS bit so ACK/NACK applied to the current byte
		I2C_AcknowledgeConfig(I2Cx, ENABLE);                             //make sure ACK is on
		index = 0;              //reset the index

		if (reading && (subaddress_sent || 0xFF == reg))                 //we have sent the subaddr
		{
			subaddress_sent = 1;                                         //make sure this is set in case of no subaddress, so following code runs correctly

			if (bytes == 2)
				I2Cx->CR1 |= 0x0800;                                     //set the POS bit so NACK applied to the final byte in the two byte read

			I2C_Send7bitAddress(I2Cx, addr, I2C_Direction_Receiver);     //send the address and set hardware mode
		}

		else                                                             //direction is Tx, or we havent sent the sub and rep start
		{
			I2C_Send7bitAddress(I2Cx, addr, I2C_Direction_Transmitter);  //send the address and set hardware mode

			if (reg != 0xFF)                                             //0xFF as subaddress means it will be ignored, in Tx or Rx mode
				index = -1;                                              //send a subaddress
		}
	}

	else if (SReg_1 & 0x0002)                                            //we just sent the address - EV6 in ref manual
	{
		//Read SR1,2 to clear ADDR
		volatile uint8_t a;
		__DMB();                                                         //memory fence to control hardware

		if (bytes == 1 && reading && subaddress_sent)                    //we are receiving 1 byte - EV6_3
		{
			I2C_AcknowledgeConfig(I2Cx, DISABLE);                        //turn off ACK
			__DMB();
			a = I2Cx->SR2;                                               //clear ADDR after ACK is turned off
			I2C_GenerateSTOP(I2Cx, ENABLE);                              //program the stop
			final_stop = 1;
			I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE);                      //allow us to have an EV7
		}

		else                                                             //EV6 and EV6_1
		{
			a = I2Cx->SR2;                                               //clear the ADDR here
			__DMB();

			if (bytes == 2 && reading && subaddress_sent)                //rx 2 bytes - EV6_1
			{
				I2C_AcknowledgeConfig(I2Cx, DISABLE);                    //turn off ACK
				I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE);                 //disable TXE to allow the buffer to fill
			}

			else if (bytes == 3 && reading && subaddress_sent)           //rx 3 bytes
				I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE);                 //make sure RXNE disabled so we get a BTF in two bytes time

			else                                                         //receiving greater than three bytes, sending subaddress, or transmitting
				I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE);
		}
	}

	else if (SReg_1 & 0x004)                                             //Byte transfer finished - EV7_2, EV7_3 or EV8_2
	{
		final_stop = 1;

		if (reading && subaddress_sent)                                  //EV7_2, EV7_3
		{
			if (bytes > 2)                                               //EV7_2
			{
				I2C_AcknowledgeConfig(I2Cx, DISABLE);                    //turn off ACK
				read_p[index++] = I2C_ReceiveData(I2Cx);                 //read data N-2
				I2C_GenerateSTOP(I2Cx, ENABLE);                          //program the Stop
				final_stop = 1;                                          //required to fix hardware
				read_p[index++] = I2C_ReceiveData(I2Cx);                 //read data N-1
				I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE);                  //enable TXE to allow the final EV7
			}

			else                                                         //EV7_3
			{
				if (final_stop)
					I2C_GenerateSTOP(I2Cx, ENABLE);                      //program the Stop

				else
					I2C_GenerateSTART(I2Cx, ENABLE);                     //program a rep start

				read_p[index++] = I2C_ReceiveData(I2Cx);                 //read data N-1
				read_p[index++] = I2C_ReceiveData(I2Cx);                 //read data N
				index++;                                                 //to show job completed
			}
		}

		else                                                             //EV8_2, which may be due to a subaddress sent or a write completion
		{
			if (subaddress_sent || (writing))
			{
				if (final_stop)
					I2C_GenerateSTOP(I2Cx, ENABLE);                      //program the Stop

				else
					I2C_GenerateSTART(I2Cx, ENABLE);                     //program a rep start

				index++;                                                 //to show that the job is complete
			}

			else                                                         //We need to send a subaddress
			{
				I2C_GenerateSTART(I2Cx, ENABLE);                         //program the repeated Start
				subaddress_sent = 1;                                     //this is set back to zero upon completion of the current task
			}
		}

		while (I2Cx->CR1 & 0x0100)
		{
			;   //we must wait for the start to clear, otherwise we get constant BTF
		}
	}

	else if (SReg_1 & 0x0040)                                            //Byte received - EV7
	{
		read_p[index++] = I2C_ReceiveData(I2Cx);

		if (bytes == (index + 3))
			I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE);                     //disable TXE to allow the buffer to flush so we can get an EV7_2

		if (bytes == index)                                              //We have completed a final EV7
			index++;                                                     //to show job is complete
	}

	else if (SReg_1 & 0x0080)                                            //Byte transmitted -EV8/EV8_1
	{
		if (index != -1)
		{
			//we dont have a subaddress to send
			I2C_SendData(I2Cx, write_p[index++]);

			if (bytes == index)                                          //we have sent all the data
				I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE);                 //disable TXE to allow the buffer to flush
		}

		else
		{
			index++;
			I2C_SendData(I2Cx, reg);                                     //send the subaddress

			if (reading || !bytes)                                       //if receiving or sending 0 bytes, flush now
				I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE);                 //disable TXE to allow the buffer to flush
		}
	}

	if (index == bytes + 1)                                              //we have completed the current job
	{
		//Completion Tasks go here
		//End of completion tasks
		subaddress_sent = 0;                                             //reset this here

		if (final_stop)                                                  //If there is a final stop and no more jobs, bus is inactive, disable interrupts to prevent BTF
			I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE);        //Disable EVT and ERR interrupts while bus inactive

		busy = 0;
	}
}

///////////////////////////////////////////////////////////////////////////////

static void i2cUnstick(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	uint8_t i;

	// SCL  PB10
	// SDA  PB11

	GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_10 | GPIO_Pin_11;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_Out_OD;

	GPIO_Init(GPIOB, &GPIO_InitStructure);

	GPIO_SetBits(GPIOB, GPIO_Pin_10 | GPIO_Pin_11);

	for (i = 0; i < 8; i++)
	{
		while (!GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_10))               // Wait for any clock stretching to finish
			delayMicroseconds(3);

		GPIO_ResetBits(GPIOB, GPIO_Pin_10);                              //Set bus low
		delayMicroseconds(3);

		GPIO_SetBits(GPIOB, GPIO_Pin_10);                                //Set bus high
		delayMicroseconds(3);
	}

	// Generate a start then stop condition

	GPIO_ResetBits(GPIOB, GPIO_Pin_11);                                  //Set bus data low
	delayMicroseconds(3);
	GPIO_ResetBits(GPIOB, GPIO_Pin_10);                                  //Set bus scl low
	delayMicroseconds(3);
	GPIO_SetBits(GPIOB, GPIO_Pin_10);                                    //Set bus scl high
	delayMicroseconds(3);
	GPIO_SetBits(GPIOB, GPIO_Pin_11);                                    //Set bus sda high
	delayMicroseconds(3);
}

///////////////////////////////////////////////////////////////////////////////

void i2cInit(I2C_TypeDef *I2C)
{
	NVIC_InitTypeDef NVIC_InitStructure;
	GPIO_InitTypeDef GPIO_InitStructure;
	I2C_InitTypeDef I2C_InitStructure;

	i2cUnstick();                                                        // clock out stuff to make sure slaves arent stuck

	// SCL  PB10
	// SDA  PB11

	GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_10 | GPIO_Pin_11;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF_OD;

	GPIO_Init(GPIOB, &GPIO_InitStructure);

	I2Cx = I2C;

	// Init I2C
	I2C_DeInit(I2Cx);
	I2C_StructInit(&I2C_InitStructure);

	I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE);                //Disable EVT and ERR interrupts - they are enabled by the first request

	I2C_InitStructure.I2C_Mode                = I2C_Mode_I2C;
	I2C_InitStructure.I2C_DutyCycle           = I2C_DutyCycle_2;
	I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
	I2C_InitStructure.I2C_ClockSpeed          = 400000;

	I2C_Init(I2Cx, &I2C_InitStructure);

	I2C_Cmd(I2Cx, ENABLE);

	// I2C ER Interrupt
	NVIC_InitStructure.NVIC_IRQChannel                   = I2C2_ER_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
	NVIC_InitStructure.NVIC_IRQChannelSubPriority        = 0;
	NVIC_InitStructure.NVIC_IRQChannelCmd                = ENABLE;

	NVIC_Init(&NVIC_InitStructure);

	// I2C EV Interrupt
	NVIC_InitStructure.NVIC_IRQChannel                   = I2C2_EV_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;

	NVIC_Init(&NVIC_InitStructure);

}

///////////////////////////////////////////////////////////////////////////////

uint16_t i2cGetErrorCounter(void)
{
	return i2cErrorCount;
}

///////////////////////////////////////////////////////////////////////////////
